/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Pedro Patron
*
*********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <osl_core/LocalCoordConverter.h>

namespace osl_core {

//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, VCS & vcs)
#else
ostream & operator << (ostream & out, VCS & vcs)
#endif
{
	out.precision(18);
   	out << "( "	<< vcs.u << ", " << vcs.v << ", " << vcs.w << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, dVCS & dvcs)
#else
ostream & operator << (ostream & out, dVCS & dvcs)
#endif
{
	out.precision(18);
   	out << "( "	<< dvcs.du << ", " << dvcs.dv << ", " << dvcs.dw << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, dVCT & dvct)
#else
ostream & operator << (ostream & out, dVCT & dvct)
#endif
{
	out.precision(18);
   	out << "( "	<< dvct.u << ", " << dvct.v << ", " << dvct.w << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, RPY & rpy)
#else
ostream & operator << (ostream & out, RPY & rpy)
#endif
{
	out.precision(18);
   	out << "( "	<< rpy.r << ", " << rpy.p << ", " << rpy.y << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, PQR & pqr)
#else
ostream & operator << (ostream & out, PQR & pqr)
#endif
{
	out.precision(18);
   	out << "( "	<< pqr.p << ", " << pqr.q << ", " << pqr.r << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, RPYT & rpyt)
#else
ostream & operator << (ostream & out, RPYT & rpyt)
#endif
{
	out.precision(18);
   	out << "( "	<< rpyt.r << ", " << rpyt.p << ", " << rpyt.y << " ) ";

	return out;
}
//***********************************************************
LocalCoordConverter::LocalCoordConverter ()
	:m_RM(3,3, 0.0), m_tRM(3,3, 0.0)
{

}
//***********************************************************
LocalCoordConverter::LocalCoordConverter (const T_NED & origin, const T_RPY & attitude)
	:m_RM(3,3, 0.0), m_tRM(3,3, 0.0)
{
	m_NEDRef	= origin;
	m_RPYRef	= attitude;

	setMatrixValues();
}


//***********************************************************
LocalCoordConverter::~LocalCoordConverter()
{

}

//***********************************************************
bool	LocalCoordConverter::isInitialised()
{
	return (! ((m_NEDRef.n == 0.0) && (m_NEDRef.e == 0.0 ) && (m_NEDRef.d == 0.0) ));
}

//***********************************************************
void	LocalCoordConverter::setReferenceRad (const T_NED & originNED, const T_RPY & attitude)
{
	m_NEDRef	= originNED;
	m_RPYRef	= attitude;

	setMatrixValues();
}
		
//***********************************************************
void	LocalCoordConverter::setReferenceDeg (const T_NED & originNED, const T_RPY & attitude)
{
	m_NEDRef	= originNED;
	
  	double deg2rad = (M_PI/180.0);

	m_RPYRef	= attitude;
	
	m_RPYRef.r	*= deg2rad;
	m_RPYRef.p	*= deg2rad;
	m_RPYRef.y	*= deg2rad;

	setMatrixValues();
}


//***********************************************************
T_NED 	LocalCoordConverter::getNEDReference (void)
{
	return m_NEDRef;
}


//***********************************************************
T_RPY 	LocalCoordConverter::getRPYRadReference (void)
{
	return m_RPYRef;
}
//***********************************************************
T_RPY 	LocalCoordConverter::getRPYDegReference (void)
{
	T_RPY	rpy;

  	double rad2deg = (180.0/M_PI);

	rpy = m_RPYRef;
	
	rpy.r	*= rad2deg;
	rpy.p	*= rad2deg;
	rpy.y	*= rad2deg;
	
	return rpy;
}


//***********************************************************
T_ENU	LocalCoordConverter::VCS2ENU (const T_VCS & vcs)
{
	T_NED ned;
	T_ENU enu;

	ned = VCS2NED(vcs);
	
	enu.e = ned.e;
	enu.n = ned.n;
	enu.u = -ned.d;

	return enu;
}


//***********************************************************
T_NED	LocalCoordConverter::VCS2NED (const T_VCS & vcs)
{
	T_NED ned;
	
  	Vector<double>	vcsV(3);
	Vector<double>	nedV(3);

	#if defined(WIN32) || defined(_WIN32)
	vcsV[0] = vcs.u;
	vcsV[1] = vcs.v;
	vcsV[2] = vcs.w;
	#else
	vcsV.setValues (3, vcs.u, vcs.v, vcs.w);
	#endif

	nedV = (m_RM) * vcsV;

	ned.n = nedV[0] + m_NEDRef.n;
	ned.e = nedV[1] + m_NEDRef.e;
	ned.d = nedV[2] + m_NEDRef.d;
			
	return ned;
}
	
//***********************************************************
T_VCS	LocalCoordConverter::ENU2VCS (const T_ENU & enu)
{
	T_NED ned;

	ned.n = enu.n;
	ned.e = enu.e;
	ned.d = -enu.u;

	return NED2VCS (ned);
}	

//***********************************************************
T_VCS	LocalCoordConverter::NED2VCS (const T_NED & ned)
{
	T_VCS	vcs;

 	Vector<double>	vcsV(3);
	Vector<double>	nedV(3);

	#if defined(WIN32) || defined(_WIN32)
	nedV[0] = ned.n - m_NEDRef.n;
	nedV[1] = ned.e - m_NEDRef.e;
	nedV[2] = ned.d - m_NEDRef.d;
	#else
	nedV.setValues(3, ned.n - m_NEDRef.n, ned.e - m_NEDRef.e, ned.d - m_NEDRef.d);
	#endif

	vcsV = m_tRM * nedV;
	
	vcs.u = vcsV[0];
	vcs.v = vcsV[1];
	vcs.w = vcsV[2];
	
	return vcs;

}

//***********************************************************
void 	LocalCoordConverter::setMatrixValues	(void)
{	
	// sin and cosines of rotation
	double cr = cos(m_RPYRef.r);
	double cp = cos(m_RPYRef.p);
	double cy = cos(m_RPYRef.y);
	double sr = sin(m_RPYRef.r);
	double sp = sin(m_RPYRef.p);
	double sy = sin(m_RPYRef.y);

	// row angle increasing clockwise 
	// pitch angle increasing clockwise
	// yaw angle increasing clockwise
	// when viewed from origin looking along
	// positive axis
	
	double r00, r01, r02, r10, r11, r12, r20, r21, r22;
	
	r00= cy*cp;		r01= -sy*cr + cy*sp*sr;		r02= sy*sr + cy*cr*sp;
	r10= sy*cp; 	r11= cy*cr + sr*sp*sy;		r12= -cy*sr + sp*sy*cr;
	r20= -sp;		r21= cp*sr;					r22= cp*cr;

	// rotation matrix
	m_RM[0][0]= r00;		m_RM[0][1]= r01;		m_RM[0][2]= r02;
	m_RM[1][0]= r10;		m_RM[1][1]= r11;		m_RM[1][2]= r12;
	m_RM[2][0]= r20;		m_RM[2][1]= r21;		m_RM[2][2]= r22;

	// inverse of rotation matrix
	m_tRM[0][0]= r00;		m_tRM[0][1]= r10;		m_tRM[0][2]= r20;
	m_tRM[1][0]= r01;		m_tRM[1][1]= r11;		m_tRM[1][2]= r21;
	m_tRM[2][0]= r02;		m_tRM[2][1]= r12;		m_tRM[2][2]= r22;
}

} // namespace
